filename = 'dataset021.yaml';

[data_alt, data_or, data_accel, data_vel] = loadDataset(filename);


%%

n=12;      %number of state

Q=[
0.001   0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.001   0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.001   0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.001   0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.001   0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.001   0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.001   0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.001   0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.001   0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.001   0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.001   0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.001
]; % covariance of process

R=[
300.0   0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     300.0   0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     100.0   0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     50.0    0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     50.0    0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     50.0    0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     50.0    0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     50.0    0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     50.0    0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
];


f=@(x,dt)[
    x(1) + dt*x(4) + 0.5*(dt^2)*x(7) ;
    x(2) + dt*x(5) + 0.5*(dt^2)*x(8) ;
    x(3) + dt*x(6) + 0.5*(dt^2)*x(9) ;
    x(4) + dt*x(7) ;
    x(5) + dt*x(8) ;
    x(6) + dt*x(9) ;
    x(7);
    x(8) ;
    x(9) ;
    x(10) ;
    x(11) ;
    x(12) ;
];  % nonlinear state equations

h=@(x,dt)[
    x(1) ;
    x(2) ;
    x(3) ;
    x(4) ;
    x(5) ;
    x(6) ;
    x(7) ;
    x(8) ;
    x(9) ;
    x(10) ;
    x(11) ;
    x(12) ;
];% measurement equation

s=[0;0;0;0;0;0;0;0;0;0;0;0];              % initial state

%x=s+q*randn(3,1); %initial state         % initial state with noise

x = s;

P = [                              % initial state covraiance
30.0    0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     30.0    0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     30.0    0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     20.0    0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     20.0    0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     20.0    0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     10.0    0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     10.0    0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     10.0    0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0 ;
0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0     0.0
];

N=length(data_alt);       % total dynamic steps
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %measurement
time = data_alt(1, 2);
dt = 0.0;

for k=1:length(data_alt)
  % time between measurements
  dt = data_alt(k, 2) - time;
  if (dt <= 0.0)
      dt = 0.0001;
  end
  time = data_alt(k, 2);

 
  s = f(s, dt);                           % update process
  z = h(s, 1);                           % measurments from predicated state
  %z = h(s, 1) + diag(R .* randn(12));     % measurments from predicated state
  z(3) = data_alt(k, 2);                  % measured altitude from dataset

  sV(:,k)= z;                             % save measurement for plot

  [x, P] = ekf(f,x,P,h,z,Q,R,dt);         % ekf 
  
  P

  xV(:,k) = x;                            % save estimate for plot
% s = f(s) + q*randn(3,1);                % update process  
end



for k=1:3                                 % plot results
  subplot(3,1,k)
  plot(data_alt(:, 1), sV(k,:), '-', data_alt(:, 1), xV(k,:), '--')
end